#include "utils.h"
#include "control/control_mag.h"
#include "spdlog/spdlog.h"
#include "spdlog/sinks/stdout_color_sinks.h"
#include "spdlog/sinks/rotating_file_sink.h"
#include "std_msgs/msg/float32_multi_array.hpp"
#include "std_msgs/msg/u_int32.hpp"

namespace control
{

Control_mag::Control_mag() 
{
	RCLCPP_INFO(node->get_logger(),"Start to load parameters._qr");
	// ros::NodeHandle private_nh_("~");
	SpdlogInit();
	#if 1
	node->declare_parameter<int>("cycle_frequency",cycle_frequency_);
	cycle_frequency_ = node->get_parameter("cycle_frequency").as_int();
	node->declare_parameter<double>("adaptive_kp_line",controlParamInfo_.adaptive_kp_line);
	controlParamInfo_.adaptive_kp_line = node->get_parameter("adaptive_kp_line").as_double();
	node->declare_parameter<double>("adaptive_ki_line",controlParamInfo_.adaptive_ki_line);
	controlParamInfo_.adaptive_ki_line = node->get_parameter("adaptive_ki_line").as_double();
	node->declare_parameter<double>("adaptive_kd_line",controlParamInfo_.adaptive_kd_line);
	controlParamInfo_.adaptive_kd_line = node->get_parameter("adaptive_kd_line").as_double();
	node->declare_parameter<double>("adaptive_kp_curve",controlParamInfo_.adaptive_kp_curve);
	controlParamInfo_.adaptive_kp_curve = node->get_parameter("adaptive_kp_curve").as_double();
	node->declare_parameter<double>("adaptive_ki_curve",controlParamInfo_.adaptive_ki_curve);
	controlParamInfo_.adaptive_ki_curve = node->get_parameter("adaptive_ki_curve").as_double();
	node->declare_parameter<double>("adaptive_kd_curve",controlParamInfo_.adaptive_kd_curve);
	controlParamInfo_.adaptive_kd_curve = node->get_parameter("adaptive_kd_curve").as_double();
	node->declare_parameter<double>("adaptive_kp_heading_line",controlParamInfo_.adaptive_kp_heading_line);
	controlParamInfo_.adaptive_kp_heading_line = node->get_parameter("adaptive_kp_heading_line").as_double();
	node->declare_parameter<double>("adaptive_ki_heading_line",controlParamInfo_.adaptive_ki_heading_line);
	controlParamInfo_.adaptive_ki_heading_line = node->get_parameter("adaptive_ki_heading_line").as_double();
	node->declare_parameter<double>("adaptive_kd_heading_line",controlParamInfo_.adaptive_kd_heading_line);
	controlParamInfo_.adaptive_kd_heading_line = node->get_parameter("adaptive_kd_heading_line").as_double();
	node->declare_parameter<double>("adaptive_kp_heading_curve",controlParamInfo_.adaptive_kp_heading_curve);
	controlParamInfo_.adaptive_kp_heading_curve = node->get_parameter("adaptive_kp_heading_curve").as_double();
	node->declare_parameter<double>("adaptive_ki_heading_curve",controlParamInfo_.adaptive_ki_heading_curve);
	controlParamInfo_.adaptive_ki_heading_curve = node->get_parameter("adaptive_ki_heading_curve").as_double();
	node->declare_parameter<double>("adaptive_kd_heading_curve",controlParamInfo_.adaptive_kd_heading_curve);
	controlParamInfo_.adaptive_kd_heading_curve = node->get_parameter("adaptive_kd_heading_curve").as_double();
	node->declare_parameter<double>("position_jump",position_jump_);
	position_jump_ = node->get_parameter("position_jump").as_double();
	node->declare_parameter<double>("heading_jump",heading_jump_);
	heading_jump_ = node->get_parameter("heading_jump").as_double();
	node->declare_parameter<double>("replan_angle_min",replan_angle_min_);
	replan_angle_min_ = node->get_parameter("replan_angle_min").as_double();
	node->declare_parameter<double>("replan_angle_max",replan_angle_max_);
	replan_angle_max_ = node->get_parameter("replan_angle_max").as_double();
	node->declare_parameter<double>("car_length",car_length_);
	car_length_ = node->get_parameter("car_length").as_double();
	node->declare_parameter<double>("car_width",car_width_);
	car_width_ = node->get_parameter("car_width").as_double();
	node->declare_parameter<bool>("navigation_estop_flag",navigation_estop_flag_);
	navigation_estop_flag_ = node->get_parameter("navigation_estop_flag").as_bool();
	node->declare_parameter<double>("estop_point_x",estop_point_x_);
	estop_point_x_ = node->get_parameter("estop_point_x").as_double();
	node->declare_parameter<double>("estop_point_y",estop_point_y_);
	estop_point_y_ = node->get_parameter("estop_point_y").as_double();
	node->declare_parameter<double>("dis_tolerance",dis_tolerance_);
	dis_tolerance_ = node->get_parameter("dis_tolerance").as_double();
	node->declare_parameter<double>("max_speed",max_speed_);
	max_speed_ = node->get_parameter("max_speed").as_double();
	node->declare_parameter<double>("max_speed_tolerance",max_speed_tolerance_);
	max_speed_tolerance_ = node->get_parameter("max_speed_tolerance").as_double();
	node->declare_parameter<double>("max_turn_speed",max_turn_speed_);
	max_turn_speed_ = node->get_parameter("max_turn_speed").as_double();
	node->declare_parameter<double>("two_line_length",two_line_length_);
	two_line_length_ = node->get_parameter("two_line_length").as_double();
	node->declare_parameter<double>("track_new_line_speed",track_new_line_speed_);
	track_new_line_speed_ = node->get_parameter("track_new_line_speed").as_double();
	node->declare_parameter<double>("max_omg",max_omg_);
	max_omg_ = node->get_parameter("max_omg").as_double();
	node->declare_parameter<double>("start_speed_limit",controlParamInfo_.start_speed_limit);
	controlParamInfo_.start_speed_limit = node->get_parameter("start_speed_limit").as_double();
	node->declare_parameter<double>("start_acc_limit",controlParamInfo_.start_acc_limit);
	controlParamInfo_.start_acc_limit = node->get_parameter("start_acc_limit").as_double();
	node->declare_parameter<double>("acc_limit",controlParamInfo_.acc_limit);
	controlParamInfo_.acc_limit = node->get_parameter("acc_limit").as_double();
	node->declare_parameter<double>("acc_limit_w",controlParamInfo_.acc_limit_w);
	controlParamInfo_.acc_limit_w = node->get_parameter("acc_limit_w").as_double();
	node->declare_parameter<double>("start_acc_limit_w",controlParamInfo_.start_acc_limit_w);
	controlParamInfo_.start_acc_limit_w = node->get_parameter("start_acc_limit_w").as_double();
	node->declare_parameter<double>("dec_limit",controlParamInfo_.dec_limit);
	controlParamInfo_.dec_limit = node->get_parameter("dec_limit").as_double();
	node->declare_parameter<double>("obs_brake_dec",controlParamInfo_.obs_brake_dec);
	controlParamInfo_.obs_brake_dec = node->get_parameter("obs_brake_dec").as_double();
	node->declare_parameter<double>("obs_estop_dec",controlParamInfo_.obs_estop_dec);
	controlParamInfo_.obs_estop_dec = node->get_parameter("obs_estop_dec").as_double();
	node->declare_parameter<double>("estop_speed_level",controlParamInfo_.estop_speed_level);
	controlParamInfo_.estop_speed_level = node->get_parameter("estop_speed_level").as_double();
	node->declare_parameter<double>("current2start_dis",current2start_dis_);
	current2start_dis_ = node->get_parameter("current2start_dis").as_double();
	node->declare_parameter<double>("current2start_dis_max",current2start_dis_max_);
	current2start_dis_max_ = node->get_parameter("current2start_dis_max").as_double();
	node->declare_parameter<double>("actuator_delaytime",controlParamInfo_.actuator_delaytime);
	controlParamInfo_.actuator_delaytime = node->get_parameter("actuator_delaytime").as_double();
	node->declare_parameter<double>("actuator_delaytime_1",controlParamInfo_.actuator_delaytime_1);
	controlParamInfo_.actuator_delaytime_1 = node->get_parameter("actuator_delaytime_1").as_double();
	node->declare_parameter<double>("actuator_delaytime_2",controlParamInfo_.actuator_delaytime_2);
	controlParamInfo_.actuator_delaytime_2 = node->get_parameter("actuator_delaytime_2").as_double();
	node->declare_parameter<double>("line_offsety",controlParamInfo_.line_offsety);
	controlParamInfo_.line_offsety = node->get_parameter("line_offsety").as_double();
	node->declare_parameter<double>("line_offsety_max",controlParamInfo_.line_offsety_max);
	controlParamInfo_.line_offsety_max = node->get_parameter("line_offsety_max").as_double();
	node->declare_parameter<double>("curve_offsety",controlParamInfo_.curve_offsety);
	controlParamInfo_.curve_offsety = node->get_parameter("curve_offsety").as_double();
	node->declare_parameter<double>("curve_offsety_max",controlParamInfo_.curve_offsety_max);
	controlParamInfo_.curve_offsety_max = node->get_parameter("curve_offsety_max").as_double();
	node->declare_parameter<double>("travel_dis_max",controlParamInfo_.travel_dis_max);
	controlParamInfo_.travel_dis_max = node->get_parameter("travel_dis_max").as_double();
	node->declare_parameter<double>("stop_dis_tolerance_straight",controlParamInfo_.stop_dis_tolerance_straight);
	controlParamInfo_.stop_dis_tolerance_straight = node->get_parameter("stop_dis_tolerance_straight").as_double();
	node->declare_parameter<double>("stop_dis_tolerance_curve",controlParamInfo_.stop_dis_tolerance_curve);
	controlParamInfo_.stop_dis_tolerance_curve = node->get_parameter("stop_dis_tolerance_curve").as_double();
	node->declare_parameter<double>("curve_insert_dis",controlParamInfo_.curve_insert_dis);
	controlParamInfo_.curve_insert_dis = node->get_parameter("curve_insert_dis").as_double();
	node->declare_parameter<double>("offsety_tolerance",controlParamInfo_.offsety_tolerance);
	controlParamInfo_.offsety_tolerance = node->get_parameter("offsety_tolerance").as_double();
	node->declare_parameter<double>("change_control_param_dis",controlParamInfo_.change_control_param_dis);
	controlParamInfo_.change_control_param_dis = node->get_parameter("change_control_param_dis").as_double();
	node->declare_parameter<double>("offsety_control_level_1",controlParamInfo_.offsety_control_level_1);
	controlParamInfo_.offsety_control_level_1 = node->get_parameter("offsety_control_level_1").as_double();
	node->declare_parameter<double>("offsety_control_level_2",controlParamInfo_.offsety_control_level_2);
	controlParamInfo_.offsety_control_level_2 = node->get_parameter("offsety_control_level_2").as_double();
	node->declare_parameter<double>("offsetangle_tolerance",controlParamInfo_.offsetangle_tolerance);
	controlParamInfo_.offsetangle_tolerance = node->get_parameter("offsetangle_tolerance").as_double();
	node->declare_parameter<double>("offsetangle_tolerance1",controlParamInfo_.offsetangle_tolerance1);
	controlParamInfo_.offsetangle_tolerance1 = node->get_parameter("offsetangle_tolerance1").as_double();
	node->declare_parameter<double>("offsetangle_control_level_1",controlParamInfo_.offsetangle_control_level_1);
	controlParamInfo_.offsetangle_control_level_1 = node->get_parameter("offsetangle_control_level_1").as_double();
	node->declare_parameter<double>("offsetangle_control_level_2",controlParamInfo_.offsetangle_control_level_2);
	controlParamInfo_.offsetangle_control_level_2 = node->get_parameter("offsetangle_control_level_2").as_double();
	node->declare_parameter<double>("creep_distance",controlParamInfo_.creep_distance);
	controlParamInfo_.creep_distance = node->get_parameter("creep_distance").as_double();
	node->declare_parameter<double>("creep_speed",controlParamInfo_.creep_speed);
	controlParamInfo_.creep_speed = node->get_parameter("creep_speed").as_double();
	node->declare_parameter<double>("arrive_target_tolerance",controlParamInfo_.arrive_target_tolerance);
	controlParamInfo_.arrive_target_tolerance = node->get_parameter("arrive_target_tolerance").as_double();
	node->declare_parameter<std::string>("path_info_", controlParamInfo_.m_path_info);
	controlParamInfo_.m_path_info = node->get_parameter("path_info_").as_string();
	#endif

	#if 0
	node->declare_parameter<int>("cycle_frequency",50);
	cycle_frequency_ = node->get_parameter("cycle_frequency").as_int();
	node->declare_parameter<double>("adaptive_kp_line",1.0);
	controlParamInfo_.adaptive_kp_line = node->get_parameter("adaptive_kp_line").as_double();
	node->declare_parameter<double>("adaptive_ki_line",0.0);
	controlParamInfo_.adaptive_ki_line = node->get_parameter("adaptive_ki_line").as_double();
	node->declare_parameter<double>("adaptive_kd_line",0.0);
	controlParamInfo_.adaptive_kd_line = node->get_parameter("adaptive_kd_line").as_double();
	node->declare_parameter<double>("adaptive_kp_curve",1.0);
	controlParamInfo_.adaptive_kp_curve = node->get_parameter("adaptive_kp_curve").as_double();
	node->declare_parameter<double>("adaptive_ki_curve",0.0);
	controlParamInfo_.adaptive_ki_curve = node->get_parameter("adaptive_ki_curve").as_double();
	node->declare_parameter<double>("adaptive_kd_curve",0.0);
	controlParamInfo_.adaptive_kd_curve = node->get_parameter("adaptive_kd_curve").as_double();
	node->declare_parameter<double>("adaptive_kp_heading_line",0.8);
	controlParamInfo_.adaptive_kp_heading_line = node->get_parameter("adaptive_kp_heading_line").as_double();
	node->declare_parameter<double>("adaptive_ki_heading_line",0.0);
	controlParamInfo_.adaptive_ki_heading_line = node->get_parameter("adaptive_ki_heading_line").as_double();
	node->declare_parameter<double>("adaptive_kd_heading_line",0.0);
	controlParamInfo_.adaptive_kd_heading_line = node->get_parameter("adaptive_kd_heading_line").as_double();
	node->declare_parameter<double>("adaptive_kp_heading_curve",0.5);
	controlParamInfo_.adaptive_kp_heading_curve = node->get_parameter("adaptive_kp_heading_curve").as_double();
	node->declare_parameter<double>("adaptive_ki_heading_curve",0.0);
	controlParamInfo_.adaptive_ki_heading_curve = node->get_parameter("adaptive_ki_heading_curve").as_double();
	node->declare_parameter<double>("adaptive_kd_heading_curve",0.0);
	controlParamInfo_.adaptive_kd_heading_curve = node->get_parameter("adaptive_kd_heading_curve").as_double();
	node->declare_parameter<double>("position_jump",0.3);
	position_jump_ = node->get_parameter("position_jump").as_double();
	node->declare_parameter<double>("heading_jump",10.0);
	heading_jump_ = node->get_parameter("heading_jump").as_double();
	node->declare_parameter<double>("replan_angle_min",3.0);
	replan_angle_min_ = node->get_parameter("replan_angle_min").as_double();
	node->declare_parameter<double>("replan_angle_max",90.0);
	replan_angle_max_ = node->get_parameter("replan_angle_max").as_double();
	node->declare_parameter<double>("car_length",0.8);
	car_length_ = node->get_parameter("car_length").as_double();
	node->declare_parameter<double>("car_width",0.6);
	car_width_ = node->get_parameter("car_width").as_double();
	node->declare_parameter<bool>("navigation_estop_flag",false);
	navigation_estop_flag_ = node->get_parameter("navigation_estop_flag").as_bool();
	node->declare_parameter<double>("estop_point_x",0.0);
	estop_point_x_ = node->get_parameter("estop_point_x").as_double();
	node->declare_parameter<double>("estop_point_y",0.0);
	estop_point_y_ = node->get_parameter("estop_point_y").as_double();
	node->declare_parameter<double>("dis_tolerance",0.1);
	dis_tolerance_ = node->get_parameter("dis_tolerance").as_double();
	node->declare_parameter<double>("max_speed",1.5);
	max_speed_ = node->get_parameter("max_speed").as_double();
	node->declare_parameter<double>("max_speed_tolerance",1.5);
	max_speed_tolerance_ = node->get_parameter("max_speed_tolerance").as_double();
	node->declare_parameter<double>("max_turn_speed",0.3);
	max_turn_speed_ = node->get_parameter("max_turn_speed").as_double();
	node->declare_parameter<double>("two_line_length",2.0);
	two_line_length_ = node->get_parameter("two_line_length").as_double();
	node->declare_parameter<double>("track_new_line_speed",0.5);
	track_new_line_speed_ = node->get_parameter("track_new_line_speed").as_double();
	node->declare_parameter<double>("max_omg",0.3);
	max_omg_ = node->get_parameter("max_omg").as_double();
	node->declare_parameter<double>("start_speed_limit",0.1);
	controlParamInfo_.start_speed_limit = node->get_parameter("start_speed_limit").as_double();
	node->declare_parameter<double>("start_acc_limit",0.1);
	controlParamInfo_.start_acc_limit = node->get_parameter("start_acc_limit").as_double();
	node->declare_parameter<double>("acc_limit",0.2);
	controlParamInfo_.acc_limit = node->get_parameter("acc_limit").as_double();
	node->declare_parameter<double>("acc_limit_w",0.2);
	controlParamInfo_.acc_limit_w = node->get_parameter("acc_limit_w").as_double();
	node->declare_parameter<double>("dec_limit",0.2);
	controlParamInfo_.dec_limit = node->get_parameter("dec_limit").as_double();
	node->declare_parameter<double>("obs_brake_dec",1.0);
	controlParamInfo_.obs_brake_dec = node->get_parameter("obs_brake_dec").as_double();
	node->declare_parameter<double>("obs_estop_dec",1.0);
	controlParamInfo_.obs_estop_dec = node->get_parameter("obs_estop_dec").as_double();
	node->declare_parameter<double>("estop_speed_level",0.5);
	controlParamInfo_.estop_speed_level = node->get_parameter("estop_speed_level").as_double();
	node->declare_parameter<double>("current2start_dis",0.1);
	current2start_dis_ = node->get_parameter("current2start_dis").as_double();
	node->declare_parameter<double>("current2start_dis_max",3.0);
	current2start_dis_max_ = node->get_parameter("current2start_dis_max").as_double();
	node->declare_parameter<double>("actuator_delaytime",0.0);
	controlParamInfo_.actuator_delaytime = node->get_parameter("actuator_delaytime").as_double();
	node->declare_parameter<double>("actuator_delaytime_1",0.0);
	controlParamInfo_.actuator_delaytime_1 = node->get_parameter("actuator_delaytime_1").as_double();
	node->declare_parameter<double>("actuator_delaytime_2",0.0);
	controlParamInfo_.actuator_delaytime_2 = node->get_parameter("actuator_delaytime_2").as_double();
	node->declare_parameter<double>("line_offsety",0.1);
	controlParamInfo_.line_offsety = node->get_parameter("line_offsety").as_double();
	node->declare_parameter<double>("line_offsety_max",0.25);
	controlParamInfo_.line_offsety_max = node->get_parameter("line_offsety_max").as_double();
	node->declare_parameter<double>("curve_offsety",0.1);
	controlParamInfo_.curve_offsety = node->get_parameter("curve_offsety").as_double();
	node->declare_parameter<double>("curve_offsety_max",0.25);
	controlParamInfo_.curve_offsety_max = node->get_parameter("curve_offsety_max").as_double();
	node->declare_parameter<double>("travel_dis_max",2.0);
	controlParamInfo_.travel_dis_max = node->get_parameter("travel_dis_max").as_double();
	node->declare_parameter<double>("stop_dis_tolerance_straight",0.3);
	controlParamInfo_.stop_dis_tolerance_straight = node->get_parameter("stop_dis_tolerance_straight").as_double();
	node->declare_parameter<double>("stop_dis_tolerance_curve",0.15);
	controlParamInfo_.stop_dis_tolerance_curve = node->get_parameter("stop_dis_tolerance_curve").as_double();
	node->declare_parameter<double>("curve_insert_dis",0.05);
	controlParamInfo_.curve_insert_dis = node->get_parameter("curve_insert_dis").as_double();
	node->declare_parameter<double>("offsety_tolerance",0.01);
	controlParamInfo_.offsety_tolerance = node->get_parameter("offsety_tolerance").as_double();
	node->declare_parameter<double>("change_control_param_dis",0.3);
	controlParamInfo_.change_control_param_dis = node->get_parameter("change_control_param_dis").as_double();
	node->declare_parameter<double>("offsety_control_level_1",0.04);
	controlParamInfo_.offsety_control_level_1 = node->get_parameter("offsety_control_level_1").as_double();
	node->declare_parameter<double>("offsety_control_level_2",0.04);
	controlParamInfo_.offsety_control_level_2 = node->get_parameter("offsety_control_level_2").as_double();
	node->declare_parameter<double>("offsetangle_tolerance",0.0174);
	controlParamInfo_.offsetangle_tolerance = node->get_parameter("offsetangle_tolerance").as_double();
	node->declare_parameter<double>("offsetangle_tolerance1",0.0087);
	controlParamInfo_.offsetangle_tolerance1 = node->get_parameter("offsetangle_tolerance1").as_double();
	node->declare_parameter<double>("offsetangle_control_level_1",2.0);
	controlParamInfo_.offsetangle_control_level_1 = node->get_parameter("offsetangle_control_level_1").as_double();
	node->declare_parameter<double>("offsetangle_control_level_2",10.0);
	controlParamInfo_.offsetangle_control_level_2 = node->get_parameter("offsetangle_control_level_2").as_double();
	node->declare_parameter<double>("creep_distance",0.1);
	controlParamInfo_.creep_distance = node->get_parameter("creep_distance").as_double();
	node->declare_parameter<double>("creep_speed",0.05);
	controlParamInfo_.creep_speed = node->get_parameter("creep_speed").as_double();
	node->declare_parameter<double>("arrive_target_tolerance",0.01);
	controlParamInfo_.arrive_target_tolerance = node->get_parameter("arrive_target_tolerance").as_double();
	node->declare_parameter<std::string>("path_info_","ubt_trajectory_track/cfg/path_info.yaml");
	controlParamInfo_.m_path_info = node->get_parameter("path_info_").as_string();
	#endif


	RCLCPP_INFO(node->get_logger(),"Paramaters loading finished.");
	// 初始化

	Init();
	setOffsetyPID();

	//location_sub_ = node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped >("/pose2d_stamped", 1, std::bind(&Control_mag::Location_QrCode_CallBack, this, std::placeholders::_1));

	agvstatus_sub_1_ = node->create_subscription<geometry_msgs::msg::Twist >("/real_speed", 1, std::bind(&Control_mag::motorInfoCallback, this, std::placeholders::_1));
	//agvstatus_sub_ = node->create_subscription<geometry_msgs::msg::Twist >("/odom", 1, std::bind(&Control_mag::agvStateInfoCallback, this, std::placeholders::_1));



	ubt_state_sub_ = node->create_subscription<std_msgs::msg::String >("/state", 10, std::bind(&Control_mag::ubtStateInfoCallback, this, std::placeholders::_1));
	stop_obs_sub_ = node->create_subscription<std_msgs::msg::UInt8 >("/nav_speed_limit", 10, std::bind(&Control_mag::stopObsCallback, this, std::placeholders::_1));
	path_load_sub_ = node->create_subscription<std_msgs::msg::String >("/path_load",10,std::bind(&Control_mag::pathLoadCallback, this, std::placeholders::_1));

	nav_cmd_sub_ = node->create_subscription<ubt_interface::msg::NavReq>("/nav_req",10, std::bind(&Control_mag::partPathInfoCallback_qr, this, std::placeholders::_1));
	nav_req_mag_sub_ = node->create_subscription<ubt_interface::msg::NavReq>("/nav_req_mag",10, std::bind(&Control_mag::partPathInfoCallback_qr, this, std::placeholders::_1));
	control_cmd_pub_ = node->create_publisher< geometry_msgs::msg::Twist >("/cmd_vel", 10);//发布命令（v,w）
	path_info_pub_   = node->create_publisher< nav_msgs::msg::Path >("/control_track_point", 10);//发布pathpoint pose
	arrivetarget_pub_= node->create_publisher< std_msgs::msg::String >("/arrived_target", 10);//发布指令执行完成，到达目标点的标志，目前已经不用

	arrivetarget_pub_qr= node->create_publisher<ubt_interface::msg::NavResp>("/nav_resp_mag", 10);//发布指令执行完成，到达目标点的标志
	timer_            = node->create_wall_timer(std::chrono::milliseconds(int((1.0/cycle_frequency_)*1000)),std::bind(&Control_mag::task_management, this));
	get_param_timer_            = node->create_wall_timer(std::chrono::milliseconds(int((1.0/10)*1000)),std::bind(&Control_mag::getParam, this));
	findRfid_timer_  = node->create_wall_timer(std::chrono::milliseconds(int((1.0/50)*1000)),std::bind(&Control_mag::TimeFindRfid, this));

	//mag
	control_mage_cmd_pub_ = node->create_publisher< std_msgs::msg::Float32MultiArray >("/mag_cmd_vel", 10);//发布控制数据，直接控制左右轮速度
	control_mage_debug_pub_ = node->create_publisher< std_msgs::msg::Float32MultiArray >("/control_mage_debug", 10);//发布调试信息
	code_mag_sub_ = node->create_subscription<std_msgs::msg::Int32MultiArray >("/magnetic_data", 1, std::bind(&Control_mag::Code_Mag_CallBack, this, std::placeholders::_1));
	max_speed_sub_ = node->create_subscription<std_msgs::msg::Float32 >("/max_speed",10,std::bind(&Control_mag::MaxSpeddCallBack, this, std::placeholders::_1));
	rfid_sub_ = node->create_subscription<ubt_interface::msg::Rfid >("/rfid_state",10,std::bind(&Control_mag::RfidCallBack, this, std::placeholders::_1));
	lift_sub_ = node->create_subscription<std_msgs::msg::UInt32 >("/traction_latch_state",10,std::bind(&Control_mag::LiftCallBack, this, std::placeholders::_1));
	characteristic_sub_ = node->create_subscription<std_msgs::msg::UInt8 >("/characteristic",10,std::bind(&Control_mag::CharacteristicCallBack, this, std::placeholders::_1));
	init_find_rfid_sub_ = node->create_subscription<std_msgs::msg::String >("/init_find_rfid",10,std::bind(&Control_mag::InitFindRfidCallBack, this, std::placeholders::_1));
	init_find_rfid_pub= node->create_publisher<std_msgs::msg::String>("/init_find_rfid_res", 10);
	control_type_pub_= node->create_publisher<std_msgs::msg::Int32>("/priority_control_type", 10);
	angle_encoder_sub_ = node->create_subscription<std_msgs::msg::Float32 >("/AngleEncoder", 10, std::bind(&Control_mag::AngleEncoderCallback, this, std::placeholders::_1));
	mdirect_cmd_vel_sub_ = node->create_subscription<std_msgs::msg::Float32MultiArray >("/direct_cmd_vel", 10, std::bind(&Control_mag::DirectCmdVelCallback, this, std::placeholders::_1));
	cmd_vel_sub_ = node->create_subscription<geometry_msgs::msg::Twist>("/cmd_vel", 10, std::bind(&Control_mag::CmdVelCallback, this, std::placeholders::_1));
	path_index_mag_pub_ = node->create_publisher< std_msgs::msg::Int32 >("/path_index_mag", 10);//
	path_index_sub_ = node->create_subscription<std_msgs::msg::Int32 >("/path_index",10,std::bind(&Control_mag::ChangePathIndexCallBack, this, std::placeholders::_1));
	standalone_mode_sub_ = node->create_subscription<std_msgs::msg::Bool >("/standalone_mode", 1, std::bind(&Control_mag::standaloneModeCallback, this, std::placeholders::_1));
}

void Control_mag::TimeFindRfid()
{
	switch(find_rfid_step_){
		case 1:
			//std::cout<<"::::::::::::::::step 1"<<std::endl;
			if(on_mag_)
			{
				find_rfid_step_=4;
				break;
			}
			if(angle_down_>-85)
			{
				PublishSpeedMsg(-0.05,0.05);
			}
			else
			{
				PublishSpeedMsg(0,0);
				find_rfid_step_=2;
			}
			break;
		case 2:
			//std::cout<<"::::::::::::::::step 2"<<std::endl;
			if(!on_mag_)
			{
				PublishSpeedMsg(0.06,0.1);
			}
			else
			{
				PublishSpeedMsg(0,0);
				find_rfid_step_=4;
			}
			break;
		case 3:
			//std::cout<<"::::::::::::::::step 3"<<std::endl;
			if(angle_down_<-0.1)
			{
				PublishSpeedMsg(0.05,-0.05);
			}
			else
			{
				PublishSpeedMsg(0,0);
				find_rfid_step_=4;
			}
			break;
		case 4:
			{
				state_value_ = 1;
				max_speed_=0.2;
				ubt_state_ = "UBT_RUN";
				ubt_state_two="RUN_";
				AgvExecuteCmdState_step_  = VmsCmdExecuteState::EXECUTING;
				RouteInfo_.clear();
				fack_path_.PathId = 0;
				fack_path_.PathType = "LINE";
				RouteInfo_.emplace_back(fack_path_);
				find_rfid_ = true;
				find_rfid_step_=0;
				std_msgs::msg::Int32 data;
				data.data = 0;
				control_type_pub_->publish(data);
				//std::cout<<"::::::::::::::::step 4"<<std::endl;
				break;
			}

		default:
			//std::cout<<"::::::::::::::::step default"<<std::endl;
			break;

	}
}

void Control_mag::standaloneModeCallback(const std_msgs::msg::Bool::SharedPtr msg)
{
	this->standalone_mode_ = msg->data ? true : false;
	std_msgs::msg::String::SharedPtr  initFindRfidMsg;
	initFindRfidMsg->data = standalone_mode_ ? "1" : "0";
	InitFindRfidCallBack(initFindRfidMsg);
}

void Control_mag::DirectCmdVelCallback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
{
	if(ubt_state_ =="UBT_RUN")
	real_send_speed_ = (msg->data.at(0) + msg->data.at(1))/2;
}


void Control_mag::CmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr speed)
{
	if(ubt_state_ =="UBT_RUN")
	real_send_speed_ = speed->linear.x;

}

void Control_mag::AngleEncoderCallback(const std_msgs::msg::Float32::SharedPtr angle)
{
	angle_down_ = angle->data;
}
void Control_mag::FindMag()
{
	while(!on_mag_)
	{
		usleep(50000);
		PublishSpeedMsg(0.1,0.2);
	}
	PublishSpeedMsg(0,0);
}
void Control_mag::TurnFoot(int way)
{
	if(way==0)
	{
		while(angle_down_>-85)
		{
			usleep(50000);
			PublishSpeedMsg(-0.05,0.05);
		}
		PublishSpeedMsg(0,0);
	}
	else if(way==1)
	{
		if(angle_down_<0)
		{

			while(angle_down_<-0.1)
		{
			usleep(50000);
			PublishSpeedMsg(0.05,-0.05);
		}
		PublishSpeedMsg(0,0);
		}
		else if(angle_down_>0)
			while(angle_down_>0.1)
		{
			usleep(50000);
			PublishSpeedMsg(-0.05,0.05);
		}
		PublishSpeedMsg(0,0);
	}
	else if(way==2)
	{
		while(angle_down_<85)
		{
			usleep(50000);
			PublishSpeedMsg(0.05,-0.05);
		}
		PublishSpeedMsg(0,0);
	}
}

void Control_mag::InitFindRfidCallBack(const std_msgs::msg::String::SharedPtr msg)
{
	std::string cmd = msg->data;
	if(cmd=="1")
	{
		find_rfid_step_=1;
		find_rfid_flag_=1;
		std_msgs::msg::Int32 data;
		data.data = 12;
		control_type_pub_->publish(data);
		if(last_rfid_is_induction_ == 1){
			find_last_rfid_ = true;
		}
		else{
			find_last_rfid_ = false;
		}
	}
	else if(cmd=="0")
	{
		find_rfid_step_=0;
		ubt_state_ = "UBT_WAIT";
		RouteInfo_.clear();
		if(find_rfid_flag_ == 1)
		{
			find_rfid_flag_=0;
			std_msgs::msg::Int32 data;
			data.data = 0;
			control_type_pub_->publish(data);
		}
	}
}


void Control_mag::CharacteristicCallBack(const std_msgs::msg::UInt8::SharedPtr msg)
{
	uint32_t cmd = msg->data;
	if(cmd==1)
	{
		characteristic_scan_flag_ = 1;
		max_speed_=0.2;
		ubt_state_ = "UBT_RUN";
		AgvExecuteCmdState_step_  = VmsCmdExecuteState::EXECUTING;
		RouteInfo_.clear();
		fack_path_.PathId = 0;
		fack_path_.PathType = "LINE";
		RouteInfo_.emplace_back(fack_path_);
	}
	else if(cmd==0)
	{
		characteristic_scan_flag_ = 0;
		ubt_state_ = "UBT_WAIT";
		RouteInfo_.clear();

	}
}

void Control_mag::ChangePathIndexCallBack(const std_msgs::msg::Int32::SharedPtr msg)
{
	int new_index = msg->data;
	if(new_index != path_index_ && AgvExecuteCmdState_step_ != VmsCmdExecuteState::WAIT && new_index >= 0)
	{
		RCLCPP_INFO(node->get_logger(),"recieive new_index ,new_index:%d,RouteInfo_.size:%d",new_index,int(RouteInfo_.size()));
		if(new_index>RouteInfo_.size()){
			RCLCPP_INFO(node->get_logger(),"recieive new_index > RouteInfo_.size.");
			return;
		}
		path_index_ = new_index;
	}
}

void Control_mag::LiftCallBack(const std_msgs::msg::UInt32::SharedPtr msg)
{
	controlParamInfo_.lift_flag=msg->data;
}

void Control_mag::RfidCallBack(const ubt_interface::msg::Rfid::SharedPtr msg)
{
	std::string newRfid=msg->id;
	// if(ubt_state_ != "UBT_RUN" && ubt_state_ != "UBT_SLOWDOWN" && ubt_state_ != "UBT_CONTINUE")
	// {
	// 	last_rfid_is_induction_ = msg.is_induction;
	// 	oldRfid = newRfid;
	// 	return;
	// }

	if(find_rfid_ == true && msg->is_induction == 1 && 
		(find_last_rfid_ == false || oldRfid != newRfid) &&
		!standalone_mode_)
	{
		AgvExecuteCmdState_step_ = VmsCmdExecuteState::FINISH;
		find_rfid_=false;
		max_speed_=0.3;
		std_msgs::msg::String first_rfid;
		first_rfid.data=msg->id;
		init_find_rfid_pub->publish(first_rfid);
		if(find_rfid_flag_ == 1)
		{
			find_rfid_flag_=0;
		}
	}
	last_rfid_is_induction_ = msg->is_induction;
	if(oldRfid == newRfid)
	{
		return;
	}
	oldRfid = newRfid;
	if(RouteInfo_.size()<1 || RouteInfo_.back().PathPointInfo.size()<1)
	{
		return;
	}

	if(end_point_flag_==1)
	{
		return;
	}
	

	int  index = 0;
	auto path = RouteInfo_.back();  //判断是否为最后一个点

	// printf("path.PathPointInfo.back().rfid:%s",path.PathPointInfo.back().rfid);
	if(path.PathType == "BESSEL" && path.PathPointInfo[1].rfid==newRfid)  //曲线
	{
		index = RouteInfo_.size();
		path_index_ = index;
		PublishPathIndexMsg(index);
		return;
	}
	else if(path.PathPointInfo.back().rfid==newRfid) 	//直线
	{
		index = RouteInfo_.size();
		path_index_ = index;
		PublishPathIndexMsg(index);
		return;
	}
	for(auto path2 = RouteInfo_.begin(); path2 != RouteInfo_.end(); ++path2)
	{
		Pose_type poseType = path2->PathPointInfo[0];
		if(poseType.rfid == newRfid)
		{
			RCLCPP_INFO(node->get_logger(),"this rfid's speed is : %lf go the path number : %d",max_speed_,target_mag_);
			path_index_ = index;
			PublishPathIndexMsg(index);
			return;
		}
		index++;
	}
	// 车辆所在rfid不在路径上，已脱离路径，急停
	if(ubt_state_ == "UBT_RUN" || ubt_state_ == "UBT_SLOWDOWN")
	{
		RCLCPP_ERROR(node->get_logger(),"Out Of RouteInfo --- AGV Estop!");
		ubt_interface::msg::NavResp respon;
		respon.status=5;  //脱轨
		arrivetarget_pub_qr->publish(respon);
		ubt_state_ == "UBT_STOP";
	}


}

void Control_mag::updatePathInfo(void)
{
	if(RouteInfo_.size()<1 || RouteInfo_.back().PathPointInfo.size()<1)
	{
		return;
	}
	if(path_index_==RouteInfo_.size()){
		end_point_flag_=1;
		max_speed_ = 0;
		if(RouteInfo_.back().PathType == "BESSEL")  //曲线
		{
			target_mag_=RouteInfo_.back().PathPointInfo[1].direction_index;
			return;
		}
		else	//直线
		{
			target_mag_=RouteInfo_.back().PathPointInfo.back().direction_index;
			return;
		}
	}else{
		Pose_type poseType = RouteInfo_[path_index_].PathPointInfo[0];
		max_speed_=poseType.max_v;
		if(max_speed_ > 1.5)
		{
			max_speed_ = 1.5;
		}
		target_mag_=poseType.direction_index;
		RCLCPP_INFO(node->get_logger(),"this path's speed is : %lf go the path number : %d",max_speed_,target_mag_);
		return;
	}

}

void Control_mag::MaxSpeddCallBack(const std_msgs::msg::Float32::SharedPtr msg)
{
	max_speed_=msg->data;
}

void Control_mag::Code_Mag_CallBack(const std_msgs::msg::Int32MultiArray::SharedPtr msg)
{
	// 函数功能：处理收到的位置信息
	if(target_mag_ < 2)
	{
		// if(fabs(offset_mag_) >0.055 && msg->data.at(target_mag_) == 0)
		// {
		// 	return;
		// }
		on_mag_ = msg->data.at(7);
		if(!on_mag_) return ;  //脱轨不更新数据
		uint32_t mag_num = msg->data.at(6);
		if(mag_num <= 1) offset_mag_ = msg->data.at(0)/1000.0;
		else offset_mag_ = msg->data.at(target_mag_)/1000.0;
		// receive_mage_time_ = rclcpp::Clock().now().seconds();
		// offset_mag_=-offset_mag_;
	}
}


void Control_mag::PublishSpeedMsg(double left,double right)
{
	std_msgs::msg::Float32MultiArray control_cmd;
	control_cmd.data.push_back(left);
	control_cmd.data.push_back(right);
	//RCLCPP_INFO(node->get_logger(),"Control--PublishSpeedMsg: publish speed,left = %f,right = %f",left,right);
	control_mage_cmd_pub_->publish(control_cmd);
}

void Control_mag::getParam()
{

	// ros::NodeHandle private_nh_("~");
	node->declare_parameter<double>("adaptive_kp_line",controlParamInfo_.adaptive_kp_line);
	controlParamInfo_.adaptive_kp_line = node->get_parameter("adaptive_kp_line").as_double();
	node->declare_parameter<double>("adaptive_ki_line",controlParamInfo_.adaptive_ki_line);
	controlParamInfo_.adaptive_ki_line = node->get_parameter("adaptive_ki_line").as_double();
	node->declare_parameter<double>("adaptive_kd_line",controlParamInfo_.adaptive_kd_line);
	controlParamInfo_.adaptive_kd_line = node->get_parameter("adaptive_kd_line").as_double();
	// RCLCPP_INFO(node->get_logger(),"adaptive_kp_line: %lf",controlParamInfo_.adaptive_kp_line);
}
void Control_mag::partPathInfoCallback_qr(const ubt_interface::msg::NavReq::SharedPtr nav_cmd_msg)
{

    int	 ubt_cmd = nav_cmd_msg->cmd;
	//将状态机的状态值进行保存
	RCLCPP_INFO(node->get_logger(),"##############get new order##############    %d",ubt_cmd);
	if (ubt_cmd==4)
	{

		state_value_ = 4;
		std::cout<<"            traffic           "<<std::endl;
		ubt_state_two="TRAFFIC_";
		RCLCPP_INFO(node->get_logger(),"ubt_state_two###############= %s",ubt_state_two.c_str());


	}
	else if (ubt_cmd==1)
	{
		state_value_ = 1;
		ubt_state_ = "UBT_RUN";
		ubt_state_two="RUN_";
		RCLCPP_INFO(node->get_logger(),"ubt_state_two###############= %s",ubt_state_two.c_str());
		std::cout<<"            run            "<<std::endl;
		spdlog::info("ubt_cmd = 1,start task.");
		AgvExecuteCmdState_step_  = VmsCmdExecuteState::EXECUTING;
		real_send_speed_ = 0;
	}
	else if (ubt_cmd==2)
	{
		state_value_ = 2;
		ubt_state_ ="UBT_PAUSE";
	}
	else if (ubt_cmd==0)
	{
		state_value_ = 0;
		ubt_state_ = "UBT_STOP";

	}
	else if (ubt_cmd==3)
	{
		state_value_ = 3;
		ubt_state_ = "UBT_CONTINUE";
	}
	else if (ubt_cmd==5)
	{
		state_value_ = 5;
		ubt_state_ = "UBT_SLOWDOWN";
	}
	else if (ubt_cmd==6)
	{
		state_value_ = 6;
		ubt_state_two="RUN_";
		// oldRfid.clear();
		RCLCPP_INFO(node->get_logger(),"ubt_state_two###############= %s",ubt_state_two.c_str());
		RCLCPP_INFO(node->get_logger(),"Control--Task : CONTINUE  RUN      mission  before ,after traffic");
		ContinueTrafficRun();
	}
	else
	{
		state_value_ = -1;
	}



    if(ubt_cmd==1)
	{
	//任务下发
    		try
		{
	/* code for Try */
				ubt_interface::msg::NavInfo pathinfo_req = nav_cmd_msg->path_info;
				//导航信息：路线序列lines
				const std::vector<ubt_interface::msg::Line> &pathlist_ =  pathinfo_req.path;
				// 解析前做一下接口路径清理
				VarResetZero();
				RouteInfo_.clear();
				RouteInfo_save.clear();
				oldRfid.clear();
				navigation_start_ = true;

				for(auto it = pathlist_.begin(); it != pathlist_.end(); ++it)
				{
					//定义算法路径接口临时保存
					PathInfoType pathInfo;
					pathInfo.PathId = 0;
					pathInfo.PathType = it->type;
					// 将it->points写入pathInfo.PathPointInfo
					GetPathPointInfo(pathInfo.PathPointInfo, it->points);
					pathInfo.v_max =it->limit.x;
					pathInfo.w_max =it->limit.w;
					RouteInfo_.emplace_back(pathInfo);
					RouteInfo_save.emplace_back(pathInfo);
				}

				// 测试：打印RouteInfo
				RCLCPP_INFO(node->get_logger(),"Time:%.2f\n [ubt_trajectory_track]: topic /nav_req  receive  cmd: %d \n RouteInfo is \n ",rclcpp::Clock().now().seconds(), 1);

				for(auto it = RouteInfo_.begin(); it != RouteInfo_.end(); ++it)
				{
						auto itp_start= it->PathPointInfo.begin();
						auto itp_end=  itp_start+1;
						RCLCPP_INFO(node->get_logger(),"pathID: %d\n PathType:%s\n direction_index:%d,start point:(%lf,%lf,%lf)\n end point: (%lf,%lf,%lf)\n",it->PathId,
						it->PathType.c_str(),
						itp_start->direction_index,
						itp_start->x,
						itp_start->y,
						itp_start->heading,
						itp_end->x,
						itp_end->y,
						itp_end->heading);
				}
		}
	// 	try
	// 	{
	// /* code for Try */
	// 			ubt_interface::msg::NavInfo pathinfo_req = nav_cmd_msg->pathInfo;

	// 			//导航信息：路线序列lines
	// 			const std::vector<ubt_interface::msg::Line> &pathlist_ =  pathinfo_req.path;
	// 			if(pathlist_.size() < 1)
	// 			{
	// 				return;
	// 			}
	// 			// 解析前做一下接口路径清理
	// 			VarResetZero();
	// 			RouteInfo_.clear();
	// 			navigation_start_ = true;

	// 			for(auto it = pathlist_.begin(); it != pathlist_.end(); ++it)
	// 			{
	// 				//定义算法路径接口临时保存
	// 				PathInfoType pathInfo;
	// 				pathInfo.PathId = 0;
	// 				pathInfo.PathType = it->type;

	// 				// 将it->points写入pathInfo.PathPointInfo
	// 				GetPathPointInfo(pathInfo.PathPointInfo, it->points);
	// 				// if(pathInfo.PathType == "BESSEL")
	// 				// {
	// 				// 	pathInfo.PathPointInfo.pop_back();
	// 				// }
	// 				pathInfo.v_max =it->limit.x;
	// 				pathInfo.w_max =it->limit.w;
	// 				RouteInfo_.emplace_back(pathInfo);
	// 			}

	// 			// 测试：打印RouteInfo
	// 			for(auto it = RouteInfo_[0].PathPointInfo.begin(); it != RouteInfo_[0].PathPointInfo.end(); ++it)
	// 			{

	// 					RCLCPP_INFO(node->get_logger(),"the nav_req get new path ,the list of rfid is : %s , max speed : %f , go : %d",it->rfid.c_str(),it->max_v,it->direction_index);
	// 			}
	// 	}
		catch (...)
		{
			/* code for Catch */
			RCLCPP_INFO(node->get_logger(),"RUN CMD faild");
		}




		}


	else if(ubt_cmd==4)
	{
	//交管
    	//const std::vector<ubt_interface::msg::Line> &path ;
		//path.push_back(nav_cmd_msg.pathInfo.path[0]);
		try
		{
			Pose_type tra_pose;
			tra_pose.rfid = nav_cmd_msg->traffic_point.rfid;
			// oldRfid.clear();
			tra_pose.direction_index = nav_cmd_msg->traffic_point.direction_index;
			tra_pose.max_v = nav_cmd_msg->traffic_point.x_vel;
			TrafficRun(tra_pose);
		}

		catch (...)
		{
			/* code for Catch */
			RCLCPP_INFO(node->get_logger(),"traffic CMD faild");
		}

	}


	Task_complete_flag_ = "NULL";

	RCLCPP_INFO(node->get_logger(),"ubt_state_ partPathInfoCallback_qr ##################=%s",ubt_state_.c_str());
}

void Control_mag::TrafficRun(Pose_type traffic_pos)
{
	RouteInfo_.clear();
	// max_speed_=save_piont.max_v;
	for(auto it = RouteInfo_save.begin(); it != RouteInfo_save.end(); ++it)
	{
		PathInfoType pathInfo;
		pathInfo.PathId = 0;
		pathInfo.PathType = it->PathType;
		// 将it->points写入pathInfo.PathPointInfo
		GetPathPointInfo_(pathInfo.PathPointInfo, it->PathPointInfo);
		pathInfo.v_max =it->v_max;
		pathInfo.w_max =it->w_max;
		RouteInfo_.emplace_back(pathInfo);
	}
	for(auto path = RouteInfo_.begin(); path != RouteInfo_.end(); ++path)
	{
		for(auto it = path->PathPointInfo.begin(); it != path->PathPointInfo.end(); ++it)
		{
			if(it->rfid==traffic_pos.rfid)
			{
				// save_piont.rfid = it->rfid;
				// save_piont.direction_index = it->direction_index;
				// save_piont.max_v = it->max_v;
				it->max_v=0;
				RCLCPP_INFO(node->get_logger(),"get traffic point ,the list of rfid is : %s , max speed : %f , go : %d",it->rfid.c_str(),it->max_v,it->direction_index);
			}
		}

	}


}

void Control_mag::ContinueTrafficRun()
{
	RCLCPP_INFO(node->get_logger(),"begin plan ContinueTrafficRun path");
	RouteInfo_.clear();
	// max_speed_=save_piont.max_v;
	for(auto it = RouteInfo_save.begin(); it != RouteInfo_save.end(); ++it)
	{
		PathInfoType pathInfo;
		pathInfo.PathId = 0;
		pathInfo.PathType = it->PathType;
		// 将it->points写入pathInfo.PathPointInfo
		GetPathPointInfo_(pathInfo.PathPointInfo, it->PathPointInfo);
		pathInfo.v_max =it->v_max;
		pathInfo.w_max =it->w_max;
		RouteInfo_.emplace_back(pathInfo);
	}
	// for(auto path = RouteInfo_.begin(); path != RouteInfo_.end(); ++path)
	// {
	// 	for(auto it = path->PathPointInfo.begin(); it != path->PathPointInfo.end(); ++it)
	// 	{
	// 		if(it->rfid==save_piont.rfid)
	// 		{
	// 			it->max_v = save_piont.max_v;
	// 			RCLCPP_INFO(node->get_logger(),"get traffic point ,the list of rfid is : %s , max speed : %f , go : %d",it->rfid.c_str(),it->max_v,it->direction_index);
	// 		}
	// 	}
	// }
}

void Control_mag::ContinuePauseRun()
{
	/*try
		{
			stop_obs_flag_=0;
			if(ubt_state_pause!="WAIT_")
				{
					ubt_state_ = "UBT_RUN";
				}
			RCLCPP_INFO(node->get_logger(),"CONTINUE CMD succeed");
		}
		catch (...)
		{
			code for Catch
			RCLCPP_INFO(node->get_logger(),"CONTINUE CMD faild");
		}
*/
}

void Control_mag::SlowDown()
{
	try
			{

				stop_obs_flag_=2;
				stop_step_=2;
				ubt_state_="UBT_RUN";


			}
			catch (...)
			{
				/* code for Catch */
				RCLCPP_INFO(node->get_logger(),"CONTINUE CMD faild");
			}

}

void Control_mag::GetPathPointInfo(std::vector<Pose_type> &pathPointInfo, const std::vector<ubt_interface::msg::Point> &pointInfo)
{
	for(auto it = pointInfo.begin(); it != pointInfo.end(); ++it)
	{
		Pose_type poseType;
		poseType.rfid = it->rfid;
		poseType.direction_index = it->direction_index;
		poseType.x = it->x;
		poseType.y = it->y;
		poseType.heading = it->t;
		poseType.max_v = it->x_vel;
		pathPointInfo.emplace_back(poseType);
	}
}

void Control_mag::GetPathPointInfo_(std::vector<Pose_type> &pathPointInfo, const std::vector<Pose_type> &pointInfo)
{
	for(auto it = pointInfo.begin(); it != pointInfo.end(); ++it)
	{
		Pose_type poseType;
		poseType.rfid = it->rfid;
		poseType.direction_index = it->direction_index;
		poseType.x = it->x;
		poseType.y = it->y;
		poseType.heading = it->heading;
		poseType.max_v = it->max_v;
		pathPointInfo.emplace_back(poseType);
	}
}

void Control_mag::SpdlogInit(void)
{
	std::string home_name = getenv("HOME");
	std::string node_name = "control_mag";
    // auto console_logger = std::make_shared<spdlog::sinks::stdout_color_sink_mt>();
    auto file_logger = std::make_shared<spdlog::sinks::rotating_file_sink_mt>(home_name + "/.ros/log/spdlog/" + node_name + "/"  + node_name + ".log",1024 * 1024 * 10, 3);
    auto logger = std::make_shared<spdlog::logger>("task_logger", spdlog::sinks_init_list({file_logger}));
    spdlog::set_default_logger(logger);
    spdlog::set_level(spdlog::level::trace);
    spdlog::flush_on(spdlog::level::err);
    spdlog::flush_every(std::chrono::seconds(1));
	// spdlog::set_pattern("[%H:%M:%S %z] [%^---%L---%$] [thread %t] %v");
	spdlog::info(">>>>>>>>>>>>>>>>>>>>>>>start<<<<<<<<<<<<<<<<<<<<<<<.");
    spdlog::info("Spdlog init success.");
}

void Control_mag::Init()
{
	RouteInfo_.clear();
	New_route_.clear();
	end_point_flag_           = 0;
	Ready_location_           = 0;
	Estop_flag_               = false;
	rotate_stop_flag_         = false;
	Task_cancel_flag_         = 0;
	stop_step_                = 0;
	path_index_               = 0;
	new_path_index_           = 0;
	stop_save_data_count_     = 0;
	stop_obs_flag_            = 0;
	turn_direction_           = 0;
	state_value_              = 0;//add 20210616增加状态机的值保存

	center_point_.x           = 0.0;
	center_point_.y           = 0.0;
	center_point_.theta       = 0.0;
	pre_center_point_.x       = 0.0;
	pre_center_point_.y       = 0.0;
	pre_center_point_.theta   = 0.0;
	centerpoint_init_.x       = 0.0;
	centerpoint_init_.y       = 0.0;
	centerpoint_init_.heading = 0.0;
	vehicle_speed_            = 0.0;
	vehicle_angular_          = 0.0;

	vehicle_motor_speed_      = 0.0;
	vehicle_motor_angular_    = 0.0;
	vehicle_odom_speed_       = 0.0;
	vehicle_odom_angular_     = 0.0;
	odom_pose_x_              = 0.0;
	odom_pose_y_              = 0.0;
	odom_pose_theta_          = 0.0;

	vel_cmd_                  = 0.0;
	w_cmd_                    = 0.0;
	target_speed_        	  = 0.0;
	init_angle_               = 0.0;
	cycletime_                = 1.0/cycle_frequency_;
	task_time_                = 0.0;
	track_time_               = 0.0;
	save_time_                = 0.0;
	dec_new_                  = 0.0;
	travel_distance_          = 0.0;//add 20210315

	read_yaml_flag_           = false;
	navigation_start_         = false;//add 20210603
	CMD_finish_flag_          = false;
	stop_save_data_flag_      = false;
	Task_complete_flag_       = "NULL";
	ubt_state_                = "UBT_WAIT";
	ubt_state_pre_            = "UBT_WAIT";
	ubt_state_two			  = "WAIT_";
	//ubt_state_pause           = "RUN__";
	AgvExecuteCmdState_step_  = VmsCmdExecuteState::EXECUTING;
	ExecutingStep_            = ExecutingStep::TurnToNewLine;
	single_path_track_step_   = SinglePathTrackStep::Track;
	track_mag_.Clear();
	new_line_tracking_.Clear();
	TraceDataCount            = 0;
	control_data_vec_.clear();
	CreatTraceFolder();
}

void Control_mag::setOffsetyPID()
{
	pid_conf_offsety_.kp  = controlParamInfo_.adaptive_kp_line;
	pid_conf_offsety_.ki  = controlParamInfo_.adaptive_ki_line;
	pid_conf_offsety_.kd  = controlParamInfo_.adaptive_kd_line;
	pid_control_offsety_.init(pid_conf_offsety_);
	pid_control_offsety_.setIntegralLimit(0.03,-0.03);
	pid_control_offsety_.setDiffMode(2);
}

void Control_mag::setHeadingPID()
{
	pid_conf_heading_.kp  = controlParamInfo_.adaptive_kp_heading_line;
	pid_conf_heading_.ki  = controlParamInfo_.adaptive_ki_heading_line;
	pid_conf_heading_.kd  = controlParamInfo_.adaptive_kd_heading_line;
	pid_control_heading_.init(pid_conf_heading_);
}

void Control_mag::locationCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr location_msg)
{
	// 函数功能：处理收到的位置信息
	geometry_msgs::msg::Pose2D car_point;
	car_point.x     = location_msg->pose.pose.position.x;
   	car_point.y     = location_msg->pose.pose.position.y;
   	tf2::Quaternion quat(0,0,location_msg->pose.pose.orientation.z,location_msg->pose.pose.orientation.w);

	tf2::Matrix3x3 m(quat);
	double roll, pitch, yaw;
	m.getRPY(roll, pitch, yaw);

   	car_point.theta = wrapToPI(yaw);

	//RCLCPP_INFO(node->get_logger(),"Control--RobotLocation: ---car_point-(%f,%f,%f)---",car_point.x,car_point.y,car_point.theta);
	location_mutex_.lock();
	center_point_      = car_point;
	location_mutex_.unlock();
	Ready_location_    = 1;
}

void Control_mag::ubtStateInfoCallback(const std_msgs::msg::String::SharedPtr ubt_state_msg)
{
	/*ubt_state_ = ubt_state_msg->data;
	//将状态机的状态值进行保存
	//1:wait,2:run,3:recharge/charge,4:obstacle,5:pause/manual_pause,6:stop,7:wait_pose,8:init_pose,-1:其他
	if (ubt_state_ == "UBT_WAIT")
	{
		state_value_ = 1;
	}
	else if (ubt_state_ == "UBT_RUN")
	{
		state_value_ = 2;
	}
	else if (ubt_state_ == "UBT_RECHARGE" || ubt_state_ == "UBT_CHARGE")
	{
		state_value_ = 3;
	}
	else if (ubt_state_ == "UBT_OBSTACLE")
	{
		state_value_ = 4;
	}
	else if (ubt_state_ == "UBT_PAUSE" || ubt_state_ == "UBT_PAUSEMANUAL")
	{
		state_value_ = 5;
	}
	else if (ubt_state_ == "UBT_STOP")
	{
		state_value_ = 6;
	}
	else if (ubt_state_ == "UBT_WAITPOSE")
	{
		state_value_ = 7;
	}
	else if (ubt_state_ == "UBT_INITPOSE")
	{
		state_value_ = 8;
	}
	else
	{
		state_value_ = -1;
	}*/
}

void Control_mag::stopObsCallback(const std_msgs::msg::UInt8::SharedPtr obstacle_msg)
{
	//stop_obs_flag_     = obstacle_msg->data;
}

void Control_mag::pathLoadCallback(const std_msgs::msg::String::SharedPtr path_load_msg)
{
	/*path_load_ = path_load_msg->data;
	//RCLCPP_INFO(node->get_logger(),"Control-pathLoadCallback: path_load_ = %s",path_load_.c_str());
	if(path_load_ == "START_LOAD")
	{
		read_yaml_flag_   = true;
		navigation_start_ = true;
	}*/
}

void Control_mag::motorInfoCallback(const geometry_msgs::msg::Twist::SharedPtr agvstatus_msg)
{
	// 函数功能：处理收到的AGV状态信息
	vehicle_motor_speed_   = agvstatus_msg->linear.x;
	vehicle_motor_angular_ = agvstatus_msg->angular.z;
	vehicle_speed_         = agvstatus_msg->linear.x;
	vehicle_angular_       = agvstatus_msg->angular.z;
}

void Control_mag::agvStateInfoCallback(const nav_msgs::msg::Odometry::SharedPtr agvstatus_msg)
{
	// 函数功能：处理收到的AGV状态信息
	vehicle_odom_speed_   = agvstatus_msg->twist.twist.linear.x;
	vehicle_odom_angular_ = agvstatus_msg->twist.twist.angular.z;
	vehicle_speed_   = agvstatus_msg->twist.twist.linear.x;
	vehicle_angular_ = agvstatus_msg->twist.twist.angular.z;
	odom_pose_x_ = agvstatus_msg->pose.pose.position.x;
	odom_pose_y_ = agvstatus_msg->pose.pose.position.y;
	tf2::Quaternion quat(0,0,agvstatus_msg->pose.pose.orientation.z,agvstatus_msg->pose.pose.orientation.w);
	tf2::Matrix3x3 m(quat);
    double roll, pitch, yaw;
   	odom_pose_theta_ = yaw;
}

void Control_mag::PublishSpeedOmgMsg(double v,double w)
{
	//发布车辆中心点v,w控制命令
	geometry_msgs::msg::Twist control_cmd;
	control_cmd.linear.x = v;
	control_cmd.linear.y = 0.0;
	control_cmd.linear.z = 0.0;

	control_cmd.angular.x = 0.0;
	control_cmd.angular.y = 0.0;
	control_cmd.angular.z = w;
	//RCLCPP_WARN(node->get_logger(),"Control--PublishSpeedOmgMsg: publish v w cmd,v = %f,w = %f",v,w);
	control_cmd_pub_->publish(control_cmd);
}

void Control_mag::PublishArrivedTargetMsg(std::string arrived_flag)
{
	std_msgs::msg::String arrived_target;
	arrived_target.data = arrived_flag;
	arrivetarget_pub_->publish(arrived_target);

}

void Control_mag::PublishPathIndexMsg(int path_index)
{
	std_msgs::msg::Int32 index;
	path_index_ = path_index;
	index.data = path_index;
	path_index_mag_pub_->publish(index);
}

void Control_mag::PublishRotateFinishMsg(int path_index)
{
	char buf[64];
	std_msgs::msg::String arrived_target;
	sprintf(buf,"TASK_ROTATE %d",path_index);
	arrived_target.data = std::string(buf);
	arrivetarget_pub_->publish(arrived_target);
}

void Control_mag::PublishBrakeMsg()
{
	w_cmd_   = 0.0;
	vel_cmd_ = vel_cmd_ - controlParamInfo_.obs_brake_dec * (1.0/cycle_frequency_);
	vel_cmd_ = vel_cmd_ <= 0.05 ? 0.0 : vel_cmd_;
	PublishSpeedOmgMsg(vel_cmd_,w_cmd_);
}

void Control_mag::PublishEstopMsg()
{
	// 下发急停命令
	// 将原有的策略直接下发速度为0，改成分段减速停车，临界速度可调 modify 20210629
	// w_cmd_ = 0.0;
	// if (vel_cmd_ > controlParamInfo_.estop_speed_level + EPSILON)
	// {

	// 	vel_cmd_ = vel_cmd_ - controlParamInfo_.obs_estop_dec * (1.0/cycle_frequency_);
	// 	vel_cmd_ = vel_cmd_ <= controlParamInfo_.estop_speed_level ? controlParamInfo_.estop_speed_level : vel_cmd_;
	// }
	// else
	// {

	// 	vel_cmd_ = vel_cmd_ - controlParamInfo_.obs_brake_dec * (1.0/cycle_frequency_);
	// 	vel_cmd_ = vel_cmd_ <= 0.05 ? 0.0 : vel_cmd_;
	// }
	// PublishSpeedOmgMsg(vel_cmd_,w_cmd_);
	PublishSpeedMsg(0.0,0.0);
}

/* void Control_mag::GetPathInfo()
{
	//read yaml file
	//指令的解析：解析yaml文件中的路径信息，得到RouteInfo_,存放之前先清空
	RouteInfo_.clear();
	std::string path_file =controlParamInfo_.m_path_info;
	//std::cout<<"start get path info yaml file"<<std::endl;
	RCLCPP_INFO(node->get_logger(),"Control-GetPathInfo: start get path info yaml file.");
	YAML::Node yamlConfig = YAML::LoadFile(path_file);
	int size = yamlConfig.size();
	std::cout<<"size = "<<size<<std::endl;
	for(int i = 0; i < size; i++)
	{
		PathInfoType pathInfo;
		pathInfo.PathPointInfo.clear();
		YAML::Node path_info_yaml = yamlConfig[i];
		pathInfo.PathId           = path_info_yaml["path"].as<int>();
		pathInfo.PathType         = path_info_yaml["type"].as<std::string>();
		//int point_size          = path_info_yaml["point_size"].as<int>();
		int point_size            = (pathInfo.PathType == "LINE" ? 2 : 3);//modify 2020102
		RCLCPP_INFO(node->get_logger(),"pount size: %d\n", point_size);
		for (int j = 0; j < point_size; j++)
		{
			Pose_type point;
			std::string name = "point"+std::to_string(j);
			YAML::Node point_info_yaml = path_info_yaml[name];
			point.x       = point_info_yaml[0].as<double>();
			point.y       = point_info_yaml[1].as<double>();
			point.heading = point_info_yaml[2].as<double>();
			pathInfo.PathPointInfo.emplace_back(point);
			RCLCPP_INFO(node->get_logger(),"---- x: %f, y:%f, z:%f", point.x,  point.y,point.heading);
		}
		pathInfo.v_max = path_info_yaml["limit_v"].as<double>();
		pathInfo.w_max = path_info_yaml["limit_w"].as<double>();
		//pathInfo.v_max = 1.5;
		//pathInfo.w_max = 1.5;
		RouteInfo_.emplace_back(pathInfo);
	}
	RCLCPP_INFO(node->get_logger(),"Control-GetPathInfo: get all path info OK.");
} */

void Control_mag::CreatTraceFolder()
{
	if(-1 == access("TraceData",0))
	{
		if(0 != mkdir("TraceData",S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH))
		{
			RCLCPP_WARN(node->get_logger(),"Control--CreatTraceFolder:Cannot Creat TraceLog Folder.");
		}
	}
	if(-1 == access("TraceData/Control",0))
	{
		if(0 != mkdir("TraceData/Control",S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH))
		{
			RCLCPP_WARN(node->get_logger(),"Control--CreatTraceFolder:Cannot Creat ControlData Folder.");
		}
	}
}

void Control_mag::TraceData()
{
	if(Task_complete_flag_ == "TASK_FINISH" || Task_complete_flag_ == "TASK_ABORT" || Task_complete_flag_ == "NULL")
	{
		stop_save_data_count_++;
		if(TraceDataCount == 0)
		{
			stop_save_data_count_ = 0;
			return;
		}
		if(stop_save_data_count_ > 20)
		{
			stop_save_data_flag_  = true;
			stop_save_data_count_ = 0;
		}
	}
	else if(Task_complete_flag_ == "TASK_WAIT")
	{
		return;
	}
	else
	{
		stop_save_data_count_ = 0;
		stop_save_data_flag_  = false;
	}

	TraceDataCount ++;

	// 需要保存的控制变量
	Pose_type pathstartpoint   = trajectorytracking_.PathStartPoint();
	Pose_type pathendpoint     = trajectorytracking_.PathEndPoint();
	Pose_type pathcontrolpoint = trajectorytracking_.PathControlPoint();
	double    w_offsety                    = trajectorytracking_.getW_offsety();
	double    w_offsetheading     = trajectorytracking_.getW_offsetheading();
	std::string control_buf = ",";
 	control_buf = control_buf +
							   std::to_string(center_point_.x) + "," +
							   std::to_string(center_point_.y) + "," +
 							   std::to_string(center_point_.theta) + "," +
							   std::to_string(vel_cmd_) + "," +
							   std::to_string(w_cmd_) + "," +
							   std::to_string(vehicle_speed_) + "," +
							   std::to_string(vehicle_angular_) + "," +
							   std::to_string(pathstartpoint.x) + "," +
							   std::to_string(pathstartpoint.y) + "," +
							   std::to_string(pathstartpoint.heading) + "," +
							   std::to_string(pathendpoint.x) + "," +
							   std::to_string(pathendpoint.y) + "," +
							   std::to_string(pathendpoint.heading) + "," +
							   std::to_string(pathcontrolpoint.x) + "," +
							   std::to_string(pathcontrolpoint.y) + "," +
							   std::to_string(pathcontrolpoint.heading) + "," +
							   std::to_string(trajectorytracking_.getOffsetY()) + "," +
							   std::to_string(trajectorytracking_.getOffsetX()) + "," +
							   std::to_string(trajectorytracking_.getOffsetHeading()) + "," +
							   std::to_string(trajectorytracking_.getOffsetSpeed()) + "," +
							   std::to_string(trajectorytracking_.TrajectoryTracking_complete()) + "," +
							   std::to_string(Ready_location_) + "," +
							   std::to_string(trajectorytracking_.getTargetDis()) + "," +
							   std::to_string(trajectorytracking_.getDecNew()) + "," +
							   std::to_string(task_time_) + "," +
							   std::to_string(save_time_) + "," +
							   std::to_string(track_time_) + "," +
							   std::to_string(vehicle_motor_speed_) + "," +
							   std::to_string(vehicle_motor_angular_) + "," +
							   std::to_string(vehicle_odom_speed_) + "," +
							   std::to_string(vehicle_odom_angular_) + "," +
							   std::to_string(odom_pose_x_) + "," +
							   std::to_string(odom_pose_y_) + "," +
							   std::to_string(odom_pose_theta_) + "," +
							   std::to_string(cos_theta_) + "," +
							   std::to_string(projection_point_.x) + "," +
							   std::to_string(projection_point_.y) + "," +
							   std::to_string(travel_distance_) + "," +
							   std::to_string(w_offsety) + "," +
							   std::to_string(w_offsetheading) + "," +
							   std::to_string(state_value_) + "," +
							   std::to_string(stop_obs_flag_) + "," +
							   std::to_string(navigation_start_) + "," +
							   std::to_string(CMD_finish_flag_) + "," +
							   std::to_string(Estop_flag_);

	control_data_vec_.push_back(control_buf);
	if((TraceDataCount == MAX_TRACE_NUM) || (Estop_flag_ == true) || (stop_save_data_flag_ == true))
	{
		//RCLCPP_INFO(node->get_logger(),"Control--TraceData: Start Save trace file .");
		CreatTraceFolder();
		time_t t = time(NULL);
		tm *local = localtime(&t);
		std::string current_time = std::to_string(local->tm_year+1900) +
									(local->tm_mon+1 < 10?("0"+std::to_string(local->tm_mon+1)):std::to_string(local->tm_mon+1)) +
									(local->tm_mday < 10?("0"+std::to_string(local->tm_mday)):std::to_string(local->tm_mday)) + "_" +
									(local->tm_hour < 10?("0"+std::to_string(local->tm_hour)):std::to_string(local->tm_hour)) + "_" +
									(local->tm_min < 10?("0"+std::to_string(local->tm_min)):std::to_string(local->tm_min)) + "_" +
									(local->tm_sec < 10?("0"+std::to_string(local->tm_sec)):std::to_string(local->tm_sec));
		std::string control_data = "TraceData/Control/ControlData_" + current_time + ".txt";
		std::ofstream control_data_file_(control_data);
		if(control_data_file_.is_open())
		{
			for(uint32_t i = 0; i < control_data_vec_.size();i++ )
			{
				control_data_file_ << control_data_vec_[i] << std::endl;
			}
			control_data_file_.close();
			//RCLCPP_INFO(node->get_logger(),"Control--TraceData: Save Control trace file OK .");
		}
		else
		{
			RCLCPP_WARN(node->get_logger(),"Control--TraceData: Create Control trace file failed.");
		}
		TraceDataCount       = 0;
		Estop_flag_          = false;
		stop_save_data_flag_ = false;
		control_data_vec_.clear();
	}
	DeleteRedundantTraceFiles();
}

void Control_mag::DeleteRedundantTraceFiles()
{
	// 遍历文件夹中的所有文件,删除文件夹中多余的文件 20201012
	DIR *dir=NULL;
	struct dirent* pDir=NULL;
	std::string Fliname_Oldest;
	// 删除决策文件中多余的文件
	int FilesNum = 0;
  	dir = opendir("TraceData/Control");
	if(dir == NULL)
	{
		RCLCPP_WARN(node->get_logger(),"Control--DeleteRedundantTraceFiles: Can't open Control dir");
		return;
	}
	while(true)
	{
		pDir = readdir(dir);
		if((pDir == NULL))
		{
			break;
		}
		if (pDir->d_type == DT_REG)
		{
			// 找出文件名最小的一个文件，即生成时间最早的一个文件
			FilesNum ++;
			if(FilesNum == 1)
			{
				Fliname_Oldest = pDir ->d_name;
			}
			else
			{
				if(Fliname_Oldest > pDir ->d_name)
				{
					Fliname_Oldest = pDir ->d_name;
				}
			}
		}
	}
	closedir(dir);
	if(FilesNum > MAX_FILE_NUM)
	{
		std::string deletfn = "TraceData/Control/" + Fliname_Oldest;
		if(-1 == remove(deletfn.c_str()))
		{
			RCLCPP_WARN(node->get_logger(),"Control--DeleteRedundantTraceFiles: delete Control trace file failed.");
			return;
		}
	}
}

void Control_mag::VarResetZero()
{
	RouteInfo_.clear();
	path_index_              = 0;
	new_path_index_          = 0;
	stop_obs_flag_           = 0;
	stop_step_               = 0;  //20210201
	dec_new_                 = 0.0;//20210201
	travel_distance_         = 0.0;//add 20210524
	vel_cmd_                 = 0.0;
	w_cmd_                   = 0.0;
	AgvExecuteCmdState_step_  = VmsCmdExecuteState::EXECUTING;
	ExecutingStep_           = ExecutingStep::TurnToNewLine;
	single_path_track_step_  = SinglePathTrackStep::Track;
}

void Control_mag::TaskWait()
{
	Task_complete_flag_  = "TASK_WAIT";
	int route_count = RouteInfo_.size();
	if (route_count > 0)
	{
		RCLCPP_WARN(node->get_logger(),"Control--Task: receive new path, start to execute");
		CMD_finish_flag_          = false;
		centerpoint_init_.x       = center_point_.x;
		centerpoint_init_.y       = center_point_.y;
		centerpoint_init_.heading = center_point_.theta;
		AgvExecuteCmdState_step_  = VmsCmdExecuteState::EXECUTING;
		Task_complete_flag_       = "TASK_EXECUTING";
	}
	else
	{
		//RCLCPP_WARN(node->get_logger(),"Control--Task: waiting new path!");
	}
	PublishArrivedTargetMsg(Task_complete_flag_);
}

void Control_mag::TaskExecuting()
{
	//RCLCPP_WARN(node->get_logger(),"Control-task_management: path_index_ = %d,RouteInfo_.size = %d,ExecutingStep_ = %d,AgvExecuteCmdState_step_ = %d,max_omg_ = %f",
	//          path_index_,int(RouteInfo_.size()),int(ExecutingStep_),int(AgvExecuteCmdState_step_),max_omg_);
	// auto now = rclcpp::Clock().now().seconds();
	// if(now - receive_mage_time_ > 0.5)
	// if(!on_mag_)
	// {
	// 	state_value_ = 0;
	// 	RouteInfo_.clear();
	// 	ubt_state_ = "UBT_STOP";
	// 	ubt_interface::msg::NavResp respon;
	// 	respon.status=4;
	// 	RCLCPP_INFO(node->get_logger(),"Control--error: out of line");
	// 	arrivetarget_pub_qr->publish(respon);
	// 	return ;
	// }
	updatePathInfo();
	TrackMagPath();


}

void Control_mag::TaskFinish()
{
	RouteInfo_.clear();
	end_point_flag_          = 0;
	stop_step_               = 0;
	ubt_state_="UBT_WAIT";
	max_speed_				 = 0.3;
	path_index_              = 0;
	Task_complete_flag_      = "TASK_FINISH";
	AgvExecuteCmdState_step_ = VmsCmdExecuteState::EXECUTING;
	RCLCPP_INFO(node->get_logger(),"Control--Task  5: all path complete,waiting new path!");
	PublishArrivedTargetMsg(Task_complete_flag_);
	ubt_interface::msg::NavResp respon;
	respon.status=3;
	RCLCPP_INFO(node->get_logger(),"Control--Task  5: send back state");
	arrivetarget_pub_qr->publish(respon);
}

void Control_mag::TrackMagPath()
{
	if(fabs(real_send_speed_ - vel_cmd_) > 0.2)
	{
		vel_cmd_ = real_send_speed_;
	}
	track_start_        = clock();
	track_mag_			= Track_mag(offset_mag_,max_speed_,vel_cmd_,cycletime_,stop_step_,pid_control_offsety_,controlParamInfo_);
	track_end_          = clock();
	track_time_         = (double)(track_end_ - track_start_) / CLOCKS_PER_SEC * 1000;
	left_speed_         = track_mag_.getLeftSpeed();
	right_speed_        = track_mag_.getRightSpeed();
	vel_cmd_            = track_mag_.getVelCmd();
	w_cmd_              = track_mag_.getWCmd();
	stop_step_          = track_mag_.getStopStep();
	if(end_point_flag_==1&&vel_cmd_<=0.02)
	{
		PublishSpeedMsg(0,0);
		AgvExecuteCmdState_step_ = VmsCmdExecuteState::FINISH;
	}
	// RCLCPP_INFO(node->get_logger(),"left : %f   right : %f",left_speed_,right_speed_);
	PublishSpeedMsg(left_speed_,right_speed_);
	std_msgs::msg::Float32MultiArray control_debug;
	control_debug.data.push_back(vel_cmd_*1000.0);
	control_debug.data.push_back(w_cmd_*1000.0);
	control_debug.data.push_back(offset_mag_*1000.0);
	control_mage_debug_pub_->publish(control_debug);
	return;
}

void Control_mag::task_management()
{
	// static rclcpp::Time old_time = rclcpp::Clock().now();
	// rclcpp::Time new_time = rclcpp::Clock().now();

	// auto time = (new_time - old_time).seconds();
	// old_time = new_time;

	// if(time > 0.02)
	// 	RCLCPP_WARN(node->get_logger(),"@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@time out 0.02:%lf", time);
	// if(time > 0.05)
	// 	RCLCPP_WARN(node->get_logger(),"@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@time out 0.05:%lf", time);
	// if(time > 0.1)
	// 	RCLCPP_WARN(node->get_logger(),"@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@time out 0.1:%lf", time);

	auto funtion_start_time = rclcpp::Clock().now().seconds();
	task_start_   = clock();
	//超时监测
	static auto last_time = rclcpp::Clock().now().seconds();
	auto now = rclcpp::Clock().now().seconds();
	if(now - last_time > 0.1f)
	{
		spdlog::warn("task_management timeout ,time interval :{}",now - last_time);
	}
	last_time = now;
	//JudgePoseJump();//对位姿跳变进行检测 delete by qli

	// if(Estop_flag_ == true)
	// {
	// 	VarResetZero();
	// 	Estop_flag_              = false;
	// 	AgvExecuteCmdState_step_ = VmsCmdExecuteState::ABORT_POSE_JUMP;
	// }
	//modify 20210512 获取路径信息
	/*if (read_yaml_flag_ == true && fabs(vel_cmd_) <= EPSILON)
	{
		//获取路径信息，读取存储路径信息的yaml文件
		VarResetZero();//add 20210524 by byf
		//GetPathInfo();
		read_yaml_flag_     = false;
		Task_complete_flag_ = "NULL";//add 20210524
	}
	else
	{
		//RCLCPP_INFO(node->get_logger(),"there is no new path ,don't need to read yaml file");
	}
*/
	//取消当前任务策略：快速减速停车，此时暂不考虑是否到达目标点
	if(ubt_state_ == "UBT_WAIT" && (ubt_state_pre_ == "UBT_RUN" || ubt_state_pre_ == "UBT_PAUSE" || ubt_state_pre_ == "UBT_PAUSEMANUAL" || ubt_state_pre_ == "UBT_OBSTACLE" || ubt_state_pre_ == "UBT_RECHARGE"))
	{
		Task_cancel_flag_ = true;
	}
	if(Task_cancel_flag_ == true)
	{
		PublishBrakeMsg();
		if(vel_cmd_ == 0.0)
		{
			Task_cancel_flag_ = false;
		}
	}
	if(ubt_state_ == "UBT_WAIT" || ubt_state_ == "UBT_ACTION")
	{
		VarResetZero();
		travel_distance_    = 0.0;//add 20210315
		Task_complete_flag_ = "NULL";
		navigation_start_   = false;
	}
	else if(ubt_state_ == "UBT_NREFAULT" || ubt_state_ == "UBT_STOP")
	{
		RCLCPP_ERROR(node->get_logger(),"Control--Task : UBT_NREFAULT or UBT_STOP --- AGV Estop!");
		PublishEstopMsg();
		stop_step_  = 0;//20210128
		dec_new_    = 0.0;
		// VarResetZero();
		Task_complete_flag_ = "TASK_ABORT";//add 20210201
		// PublishArrivedTargetMsg(Task_complete_flag_);//add 20210201
		if(fabs(vehicle_speed_) < 0.02)
		{
			RouteInfo_.clear();
			end_point_flag_          = 0;
			stop_step_               = 0;
			ubt_state_="UBT_WAIT";
			max_speed_				 = 0.3;
			path_index_              = 0;
		}
	}
	else if(ubt_state_ == "UBT_PAUSE" || ubt_state_ == "UBT_PAUSEMANUAL")
	{
		RCLCPP_WARN(node->get_logger(),"Control--Task : UBT_PAUSE  or UBT_PAUSEMANUAL --- AGV Brake!");
		stop_obs_flag_=1;
		//PublishEstopMsg();
		stop_step_  = 1;//20210128
		dec_new_    = 0.0;
		ubt_state_ = "UBT_RUN";
	}
	else if( ubt_state_ == "UBT_CONTINUE" )
	{
			//ContinuePauseRun();
	try
		{

			stop_obs_flag_=0;
			stop_step_=0;
			ubt_state_ = "UBT_RUN";
			RCLCPP_INFO(node->get_logger(),"CONTINUE CMD succeed");
		}
		catch (...)
		{
			/* code for Catch */
			RCLCPP_INFO(node->get_logger(),"CONTINUE CMD faild");
		}
			RCLCPP_INFO(node->get_logger(),"Control--Task : CONTINUE  RUN   before   mission,after pause");
	}

	else if(ubt_state_ == "UBT_TRAFFIC_CONTINUE" )
	{

	}

	else if(ubt_state_ == "UBT_SLOWDOWN")
	{
		if(ubt_state_two!="WAIT_")
		{
			SlowDown();
			RCLCPP_INFO(node->get_logger(),"Control--Task : CONTINUE  RUN   ");
		}
	}
	// else if( ( ubt_state_ == "UBT_RUN"&&int(RouteInfo_.size())>0 ) || ubt_state_ == "UBT_RECHARGE"||(ubt_state_ == "UBT_TRAFFIC"&&int(RouteInfo_.size())>0) )
	else if ( ubt_state_ == "UBT_RUN"&&int(RouteInfo_.size())>0)
	{
		// RCLCPP_INFO(node->get_logger(),"ubt_state_ : UBT_RUN");
		switch (AgvExecuteCmdState_step_)
		{

			case VmsCmdExecuteState::EXECUTING:
				{
					// RCLCPP_INFO(node->get_logger(),"Control--Task : EXECUTING");
					TaskExecuting();
				}
				break;
			case VmsCmdExecuteState::FINISH:
				{
					TaskFinish();
				}
				break;
			default:
				RCLCPP_INFO(node->get_logger(),"VmsCmdExecuteState :%d!",AgvExecuteCmdState_step_);
				break;
		}
	 }
	ubt_state_pre_ = ubt_state_;
}

}


